/* 三自由度 IK */
#ifndef IK_H
#define IK_H

#include <eigen3/Eigen/Dense>
#include "motor_ctrl.h"

#define POS_MAX 4
#define POS_01  0
#define POS_12  1
#define POS_23  2
#define POS_03  3

/* 关节长度定义 */
#define L_ABCD      (6.35f)
#define L_HIP       (20.0f)
#define L_KNEE      (15.0f)         //(15.0274f)

/* 逆运动学的关节角度限位 */
#define THETA0_MIN (-45.0f)
#define THETA0_MAX (45.0f)

#define THETA1_MIN (0.0f)
#define THETA1_MAX (90.0f)

#define THETA2_MIN (-180.0f)
#define THETA2_MAX (-90.0f)

#define THETA_ERROR (0.1f)

class IK_Motor_Param {
public :
    /* 端点位置 */
    double pos[3] = {0.0f};              
    /* 角度 */
    double angle[3] = {0.0f};
    /* 站在结构的角度上，根据实际安装的电机，表达运动学角度正方向与电机角度正方向的系数关系 */
    Dir_Type theta_dir[3] = {DIR_COUNTER, DIR_COUNTER, DIR_COUNTER};
    /* 零点时刻，运动学角度 和 电机角度 之间的 角度差 */
    double theta_offset[3] = {0.0f, 0.0f, -90.0f};
};

class IK_Obj {
public:
    IK_Obj(Motor_Ctrl *ctrl, double l_abcd, double l_hip, double l_knee);
    ~IK_Obj();
    int ik_init() {return 0;}
    int ik_deinit() {return 0;}
    void forward_kinematics();
    void inverse_kinematics();
    inline IK_Motor_Param *get_ik_param() {
        return &ik_param;
    }
    inline double get_labcd(){return L_abcd;}
    inline double get_lhip(){return L_hip;}
    inline double get_lknee(){return L_knee;}
    uint32_t is_angle_valid();
private:
    Motor_Ctrl *pmotor_ctrl = nullptr;
    IK_Motor_Param ik_param;
    double L_abcd = 0;
    double L_hip = 0;
    double L_knee = 0;
};



#endif